/*******************************************************************************
 * Copyright (C) 2013 Jeremy Brodt and Kevin Gillespie
 * Description: IMU
 *         $Id: imu.c 6 2013-12-18 13:57:11Z brodtj1212@gmail.com $
 *******************************************************************************
 */

#include "maxq_config.h"
#include "comm/maxq_comm.h"
#include "imu.h"
#include "MPU6050.h"

#define CYCLES_TO_CAL   8

int16_t ax, ay, az, gx, gy, gz;
int16_t axi, ayi, azi, gxi, gyi, gzi;
static int32_t axil, ayil, azil, gxil, gyil, gzil;
static int32_t axo, ayo, azo, gxo, gyo, gzo;
static int initcnt;

/******************************************************************************/
int imu_init(void)
{
  ax = 0;
  ay = 0;
  az = 0;
  gx = 0;
  gy = 0;
  gz = 0;
  axo = 0;
  ayo = 0;
  azo = 0;
  gxo = 0;
  gyo = 0;
  gzo = 0;
  axi = 0;
  ayi = 0;
  azi = 0;
  gxi = 0;
  gyi = 0;
  gzi = 0;
  axil = 0;
  ayil = 0;
  azil = 0;
  gxil = 0;
  gyil = 0;
  gzil = 0;
  initcnt = 0;

  return MPU_init();
}

/******************************************************************************/
int imu_task(void)
{
  int16_t axl, ayl, azl, gxl, gyl, gzl;

  getMotion6(&axl, &ayl, &azl, &gxl, &gyl, &gzl);

  if (initcnt < CYCLES_TO_CAL) {
#if 1
    axo += axl;
    ayo += ayl;
    azo += azl;
    gxo += gxl;
    gyo += gyl;
    gzo += gzl;
#endif
    initcnt++;

#if 1
    if (initcnt == CYCLES_TO_CAL) {
      axo = (axo + 4) >> 3;
      ayo = (ayo + 4) >> 3;
      azo = (azo + 4) >> 3;
      gxo = (gxo + 4) >> 3;
      gyo = (gyo + 4) >> 3;
      gzo = (gzo + 4) >> 3;
      maxq_printf(C"%7d %7d %7d", axo, ayo, azo);
      maxq_printf(C" : %7d %7d %7d\r\n", gxo, gyo, gzo);
    }
#endif
  }

  if (initcnt >= CYCLES_TO_CAL) {
    ax = axl - (int16_t)axo;
    ay = ayl - (int16_t)ayo;
    az = azl - (int16_t)azo;
    gx = gxl - (int16_t)gxo;
    gy = gyl - (int16_t)gyo;
    gz = gzl - (int16_t)gzo;
    axil += axl;
    ayil += ayl;
    azil += azl;
    gxil += gxl;
    gyil += gyl;
    gzil += gzl;
    axi = (axil + 0x80) >> 8;
    ayi = (ayil + 0x80) >> 8;
    azi = (azil + 0x80) >> 8;
    gxi = (gxil + 0x80) >> 8;
    gyi = (gyil + 0x80) >> 8;
    gzi = (gzil + 0x80) >> 8;
  }

  return 0;
}
